//------------------------------------------------------------------------------
//  <copyright file="FriServer.h" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------	

#pragma once

#include <friremote.h>

#include "stdafx.h"
#include "FRIInterop.h"
#include "IFRIConnection.h"
#include "IFRIRemote.h"
#include <KukaLBR4Definitions.h>

/// <summary> Implementation of FRI for LBR4+. </summary>
class KUKALBR4PLUSCONTROLLER_DLLEXPORTIMPORT FRIServer : public IFRIConnection
{
public:
	/// <summary> Initializes a new instance of the <c>FRIServer</c> class.  Uses KUKA FRI remote class. </summary>
	FRIServer();

	/// <summary> Initializes a new instance of the <c>FRIServer</c> class.  Uses provided FRI remote object. </summary>
	/// <param name="pFRIRemoteIn"> The FRI remote object to use. </param>
	FRIServer(IFRIRemote *pFRIRemoteIn);

	/// <summary> dtor. </summary>
	~FRIServer();

        /// <summary> Initialize state </summary>
	void Initialize();

	/// <summary> Close the FRI server. </summary>
	/// <returns> Status code.  0 on no error. </returns>
	virtual int CloseFRI();

	/// <summary> Blocking wait on connection by FRI client </summary>
	/// <returns> Status code.  0 on no error. </returns>
	virtual int WaitOnFRIConnect();

	/// <summary> Blocking wait on request from FRI client (all control actions are clocked by client). </summary>
	/// <param name="ppFeedbackRegisters">
	/// Array of arrays of type double corresponding to the feedback registers.  Current FRI feedback
	/// will be placed in these registers.
	/// </param>
	/// <returns> Status code.  0 on no error. </returns>
	virtual int GetFRIRequest(double **ppFeedbackRegisters);
	
	/// <summary> Gets the quality of the FRI connection </summary>
	virtual FRIQuality GetFRIQuality();

	/// <summary> Respond to client but take no control action. </summary>
	/// <returns> Status code.  0 on no error. </returns>
	virtual int DoNothing();

	/// <summary> Execute joint position control. </summary>
	/// <param name="pJointPosition"> Array of joint positions in order from proximal to distal. </param>
	/// <returns> Status code.  0 on no error. </returns>
	virtual int DoPositionControl(double *pJointPosition);

	/// <summary> Execute joint impedance control. </summary>
	/// <param name="pJointPosition"> Array of joint positions in order from proximal to distal. </param>
	/// <param name="pJointStiffness"> Array of joint stiffness values in order from proximal to distal. </param>
	/// <param name="pJointDamping"> Array of joint damping values in order from proximal to distal. </param>
	/// <param name="pJointAdditionalTorque"> Array of addition joint torque values in order from proximal to distal. </param>
	/// <returns> Status code.  0 on no error. </returns>
	virtual int DoJointImpedanceControl(
		double *pJointPosition,
		double *pJointStiffness,
		double *pJointDamping,
		double *pJointAdditionalTorque);

	/// <summary> Execute Cartesian impedance control. </summary>
	/// <param name="pCartesianPosition"> Array indicating desired cartesian position of TCP (tool control point) relative to base frame. </param>
	/// <param name="pCartesianStiffness"> Array of Cartesian stiffness values in frame of TCP (tool control point). </param>
	/// <param name="pCartesianDamping"> Array of Cartesian damping values in frame of TCP (tool control point). </param>
	/// <param name="pAdditionalTCPForceTorque"> Array of additional TCP force torque values. </param>
	virtual int DoCartesianImpedanceControl(
		double *pCartesianPosition,
		double *pCartesianStiffness,
		double *pCartesianDamping,
		double *pAdditionalTCPForceTorque);

	/// <summary> Execute Cartesian impedance control by passing position goals to KRL script. </summary>
	/// <param name="pCartesianPosition"> Array indicating desired cartesian position of TCP (tool control point) relative to base frame. </param>
	/// <param name="sequenceNumber"> Used to identify when a new position command is ready to be executed. </param>
	virtual int DoKrlCartesianImpedanceControl(double *pCartesianPosition, int sequenceNumber);

private:
	/// <summary> FRI remoting implementation. </summary>
	IFRIRemote *pFRIRemote;

	/// <summary> Stores the last joint command sent by the server. </summary>
	double lastJointCommandSent[NUM_JOINTS];
};
